#!/usr/bin/env python3
# coding:utf-8
import time
import cv2
import numpy as np
import serial
import math
import rospy
import queue
import threading
from std_msgs.msg import Int8
def nothing(x):
    pass



class VideoCapture:
    """Customized VideoCapture, always read latest frame"""
    
    def __init__(self, name):
        self.cap = cv2.VideoCapture(name)
        self.q = queue.Queue(maxsize=3)
        self.stop_threads = False    # to gracefully close sub-thread
        th = threading.Thread(target=self._reader)
        th.daemon = True             # 设置工作线程为后台运行
        th.start()

    # 实时读帧，只保存最后一帧
    def _reader(self):
        while not self.stop_threads:
            ret, frame = self.cap.read()
            if not ret:
                break
            if not self.q.empty():
                try:
                    self.q.get_nowait() 
                except queue.Empty:
                    pass
            self.q.put(frame)

    def read(self):
        return self.q.get()
    
    def terminate(self):
        self.stop_threads = True
        self.cap.release()
        
def findbigestCircle(frame_gray, frame, n, m):
    circles = cv2.HoughCircles(frame_gray, cv2.HOUGH_GRADIENT, 1, 90, param1=100, param2=25, minRadius=n, maxRadius=m)
    n = 0
    num = []
    if circles is not None:
        circles = np.uint16(np.around(circles))
        #print(circles[0])

        for i in circles[0, :]:
            cv2.circle(frame, (i[0], i[1]), i[2], (0, 255, 0), 2)
            cv2.circle(frame, (i[0], i[1]), 2, (0, 255, 0), -1)
            str1 = '(' + str(i[0]) + ',' + str(i[1]) + ')'
            # cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
            # cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 0), 1)
            cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
                        cv2.FONT_HERSHEY_COMPLEX, 0.6, (0,
                                                        255, 0), 1)
            #print("({}, {})".format(i[0], i[1]))
            number = [i[0], i[1]]
            n = 1
            return number
    if n != 1:
        return num


def findsmallestCircle(frame_gray, frame):
    circles = cv2.HoughCircles(frame_gray, cv2.HOUGH_GRADIENT, 1, 90, param1=100, param2=25, minRadius=20, maxRadius=80)
    n = 0
    num = []
    if circles is not None:
        circles = np.uint16(np.around(circles))
        #print(circles[0])

        for i in circles[0, :]:
            cv2.circle(frame, (i[0], i[1]), i[2], (0, 0, 255), 2)
            cv2.circle(frame, (i[0], i[1]), 2, (0, 0, 255), -1)
            str1 = '(' + str(i[0]) + ',' + str(i[1]) + ')'
            # cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
            # cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 0), 1)
            cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
                        cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0,
                                                        255), 1)
            #print("({}, {})".format(i[0], i[1]))
            number = [i[0], i[1]]
            n = 1
            return number
    if n != 1:
        return num
def hit_cb(data):
    global flag
    if(data.data==1):
        flag=1
    else:
        flag=0


def centerPoint(image, cnt):
    # loop over the contours
        # compute the center of the contour
    number = []
    for c in cnt:
        M = cv2.moments(c)
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])
        route = math.sqrt (((cX - 250) ** 2) + ((cY- 250) ** 2))
        cns = cv2.contourArea(c)
        #print(f"mianji{cns}")
        if route<200 and cns>=100:

            x, y, w, h = cv2.boundingRect(c)
            cv2.rectangle(warped, (x, y), (x + w, y + h), (0, 0, 0), -1)

            #print(f"({cX}, {cY})")
            # draw the contour and center of the shape on the image
            # cv2.drawContours(image, [c], -1, (0, 255, 0), 2)
            # cv2.circle(image, (cX, cY), 7, (255, 255, 255), -1)
            cv2.putText(image, f"({cX}, {cY})", (cX - 20, cY - 20),
            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
            number = [cX, cY]
   #print(number)
    return image, number
        # show the image

countr=0
county=0
if __name__ == "__main__":
    cap = cv2.VideoCapture(5)
    ser=serial.Serial('/dev/ttyS0',115200)
    yaw=70
    roll=80
    lastrollerr=0
    lastyawerr=0
    yawi=0
    rolli=0
    green=1
    lastsendtime=0
    flag=0
    lasttestsend=time.time()
    rospy.init_node('hit', anonymous=True)
    rospy.Subscriber("/hit",Int8,hit_cb)
    # cv2.namedWindow('Canny')
    # cv2.namedWindow('imge')
    # cv2.namedWindow('erode')
    # cv2.createTrackbar('max', 'Canny', 1, 255, nothing)
    # cv2.createTrackbar('mix', 'Canny', 1, 255, nothing)
    # cv2.namedWindow('dilate')
    # cv2.createTrackbar('n', 'dilate', 1, 20, nothing)
    # cv2.createTrackbar('m', 'erode', 1, 20, nothing)
    # cv2.createTrackbar('max', 'imge', 1, 200, nothing)
    # cv2.createTrackbar('mix', 'imge', 1, 50, nothing)
    # cv2.setTrackbarPos('mix', 'Canny', 210)
    # cv2.setTrackbarPos('max', 'Canny', 255)
    # cv2.setTrackbarPos('mix', 'imge', 0)
    # cv2.setTrackbarPos('max', 'imge', 20)
    cv2.namedWindow('pidcontrol')
    cv2.createTrackbar('roll_p', 'pidcontrol', 20, 100, nothing)
    cv2.createTrackbar('roll_i', 'pidcontrol', 0, 100, nothing)
    cv2.createTrackbar('roll_d', 'pidcontrol', 7, 100, nothing)
    cv2.createTrackbar('yaw_p', 'pidcontrol', 30, 100, nothing)
    cv2.createTrackbar('yaw_d', 'pidcontrol', 7, 100, nothing)
    cv2.createTrackbar('yaw_i', 'pidcontrol', 0, 100, nothing)
    lhsv1 = np.array([0, 43, 46])
    lhsv2 = np.array([156, 43, 46])
    hhsv1 = np.array([10, 255, 255])
    hhsv2 = np.array([180, 255, 255])
    wlhsv = np.array([0, 0, 221])
    whhsv = np.array([180, 30, 255])
    width, height = 500, 500
    l_lastrollerr=0
    l_lastyawerr=0
    times=0
    rollsum=0
    start = time.time()
    lastsendtime=time.time()
    cny = 0
    #ser.write('100Y'.encode())
    while not rospy.is_shutdown():
        result3=[]
        #time.sleep(0.1)
        frame = cap.read()[1]
        #imge = frame.copy()
        #img1 = frame.copy()
        img = cv2.resize(frame, (640, 480), interpolation=cv2.INTER_AREA)
        #imge = cv2.resize(imge, (640, 480), interpolation=cv2.INTER_AREA)

        imge=img.copy()
        cny += 1
        # mix = cv2.getTrackbarPos('mix', 'Canny')
        # max = cv2.getTrackbarPos('max', 'Canny')
        # n = cv2.getTrackbarPos('n', 'dilate')
        # m = cv2.getTrackbarPos('m', 'erode')
        roll_p = cv2.getTrackbarPos('roll_p', 'pidcontrol')/100.0
        roll_d = cv2.getTrackbarPos('roll_d', 'pidcontrol')/100.0
        roll_i = cv2.getTrackbarPos('roll_i', 'pidcontrol')/100.0
        yaw_p = cv2.getTrackbarPos('yaw_p', 'pidcontrol')/100.0
        yaw_d = cv2.getTrackbarPos('yaw_d', 'pidcontrol')/100.0
        yaw_i = cv2.getTrackbarPos('yaw_i', 'pidcontrol')/100.0
        simple_roll_p = 1.6#cv2.getTrackbarPos('roll_p', 'pidcontrol')/10.0
        simple_roll_d = 0.1#cv2.getTrackbarPos('roll_d', 'pidcontrol')/100.0
        simple_roll_i = 0#cv2.getTrackbarPos('roll_i', 'pidcontrol')/100.0
        simple_yaw_p = 3.1#cv2.getTrackbarPos('yaw_p', 'pidcontrol')/10.0
        simple_yaw_d = 0.07#cv2.getTrackbarPos('yaw_d', 'pidcontrol')/100.0
        simple_yaw_i = 0#cv2.getTrackbarPos('yaw_i', 'pidcontrol')/100.0

        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        Canny = cv2.Canny(gray, 210, 255)
        result = findbigestCircle(Canny, imge, 0, 20)
        if flag == 1:
            grid_HSV = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)
            mask1 = cv2.inRange(grid_HSV, lhsv1, hhsv1)
            mask2 = cv2.inRange(grid_HSV, lhsv2, hhsv2)
            mask = mask1 + mask2
            kernel = np.ones((3, 3), np.uint8)
            mask = cv2.dilate(mask, kernel, iterations = 5)
            # cv2.imshow("mask", mask)

            # Canny = cv2.Canny(gray, 210, 255)
            # dilate = cv2.dilate(Canny, kernel, iterations=1)
            # erode = cv2.erode(dilate, kernel, iterations=1)
            # cv2.imshow("Canny", Canny)
            # cv2.imshow('dilate', dilate)
            # cv2.imshow('erode', erode)
            # mam = cv2.getTrackbarPos('max', 'e')
            # mim = cv2.getTrackbarPos('mix', 'imge')
            # result = findbigestCircle(Canny, imge, 0, 20)#whole
            contours = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                        cv2.CHAIN_APPROX_SIMPLE)[1]

            # if len(contours) > 0:
            cnts = sorted(contours, key=cv2.contourArea, reverse=True)

        # cv2.drawContours(img, [cnts[0]], -1, (0, 255, 0), 3)
        #透视变换
            if len(cnts) != 0 :
                x, y, w, h = cv2.boundingRect(cnts[0])
                # 在原图上画出预测的矩形
                # cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 255), 1)
                rect = np.array([
                    [x, y],
                    [x + w, y],
                    [x + w, y + h],
                    [x, y + h]], dtype="float32")
                dst = np.array([
                    [0, 0],
                    [width, 0],
                    [width, height],
                    [0, height]], dtype="float32")
                ping = cv2.getPerspectiveTransform(rect, dst)
                warped = cv2.warpPerspective(img, ping, (width, height))
                # warped = cv2.resize(warped, (500, 500), interpolation=cv2.INTER_AREA)
                warped_HSV = cv2.cvtColor(warped, cv2.COLOR_BGR2HSV)
                w_mask = cv2.inRange(warped_HSV, wlhsv, whhsv)
                w_mask = cv2.erode(w_mask, kernel, iterations=2)
                w_mask = cv2.dilate(w_mask, kernel, iterations=6)
                contour = cv2.findContours(w_mask, cv2.RETR_EXTERNAL,
                                        cv2.CHAIN_APPROX_SIMPLE)[1]
                # cnt = sorted(contour, key = cv2.contourArea)
                
                if len(contour) != 0:
                    
                    # x, y, w, h = cv2.boundingRect(contour[0])
                    # cv2.rectangle(warped, (x, y), (x + w, y + h), (0, 0, 0), 1)
                    warped, result3 = centerPoint(warped, contour)
                    # cv2.drawContours(warped, contour, -1, (0, 255, 0), 3)
                    mask3 = cv2.inRange(warped_HSV, lhsv1, hhsv1)
                    mask4 = cv2.inRange(warped_HSV, lhsv2, hhsv2)
                    newmask = mask3 + mask4
                    Canny_new = cv2.Canny(newmask, 20, 100)
                    result2 =findsmallestCircle(Canny_new, warped)#part
                    # print(warped.shape)
                    #cv2.imshow("w_mask", w_mask)
                    #cv2.imshow("newmask", newmask)
                    #cv2.imshow("Canny_new", Canny_new)
                    #print(result3)
                #cv2.imshow("warped", warped)
            #end of 透视变换

        #pid计算
        if(len(result3)==2 and len(result2)==2 and green==1):#激光瞄准
            #test
            
            rollerr = 250- result3[1]
            yawerr = 255 - result3[0]
            if(time.time()-lasttestsend>1):
                if(abs(rollerr)>10):
                    test_roll=int(roll+rollerr/30.0)
                    roll=test_roll
                else:
                    test_roll=roll
                if(abs(yawerr)>10):
                    test_yaw=int(yaw-yawerr/50.0)
                    yaw=test_yaw
                else:
                    test_yaw=yaw
                test_roll_send=str(test_roll // 100) + str((test_roll % 100) // 10) + str(test_roll % 10) + "R"
                test_yaw_send=str(test_yaw // 100) + str((test_yaw % 100) // 10) + str(test_yaw % 10) + "Y"
                lasttestsend=time.time()
                ser.write(test_roll_send.encode())
                ser.write(test_yaw_send.encode())
            if(abs(rollerr)<20 and abs(yawerr)<32 and countr<10):
                countr+=1
            if(countr>=5):
                print("hit OK!!!!!!!!!!!!!!!!!!!!!!!!!!")
                ser.write("000Z".encode())
            print("{} {} {} {}".format(rollerr,roll,yawerr,yaw))
        else:
            if (len(result) == 2):#普通瞄准
                # 俯仰角计算
                maxrollout=7
                rollerr = 188 - result[1]
                yawerr = 296 - result[0]
                if(abs(rollerr<10)and abs(yawerr)<10):
                    lastyawerr=yawerr
                    lastrollerr=rollerr
                    continue
                if(abs(lastrollerr-rollerr)>90 or abs(lastyawerr-yawerr)>90):
                    lastyawerr=yawerr
                    lastrollerr=rollerr
                    continue
                p_out=rollerr / 100.0 * simple_roll_p
                d_out=(rollerr - lastrollerr) * simple_roll_d
                if(d_out>7):
                    d_out=7
                if(d_out<-7):
                    d_out=-7
                rollout=p_out+d_out
                rollout=int(rollout)
                if(rollout>maxrollout):
                    rollout=maxrollout
                if(rollout<-maxrollout):
                    rollout=-maxrollout
                roll -= rollout
                if (roll < 30):
                    roll = 30
                if (roll > 140):
                    roll = 140
                #if(abs(rollout)>3):
                    #print("roll: err:{} p:{:4f} d:{:4f} out:{}".format(rollerr,p_out,d_out,rollout))
                senddata_roll = str(roll // 100) + str((roll % 100) // 10) + str(roll % 10) + "R"
                #print(rollerr)
                

                # 偏航角计算yaw（new）
                maxyawout = 7

                p_out=yawerr / 100.0 * simple_yaw_p
                d_out=(yawerr - lastyawerr) * simple_yaw_d
                if(p_out>4):
                    p_out=4
                if(p_out<-4):
                    p_out=-4
                if(d_out>3):
                    d_out=3
                if(d_out<-3):
                    d_out=-3
                if(abs(yawerr)<50):
                    p_out=0 
                yawout=p_out+d_out
                yawout=int(yawout)
                if (yawout > maxyawout):
                    yawout = maxyawout
                if (yawout < -maxyawout):
                    yawout = -maxyawout

                yaw += yawout
                if (yaw < 20):
                    yaw = 20
                if (yaw > 120):
                    yaw = 120
                #if(abs(yawout)>3):
                #print("yaw: err:{} p:{:4f} d:{:4f} out:{}".format(yawerr,p_out,d_out,yawout))
                senddata = str(yaw // 100) + str((yaw % 100) // 10) + str(yaw % 10) + "Y"
                #print(yawerr)
                if(time.time()-lastsendtime>0.1):
                    ser.write(senddata_roll.encode())
                    ser.write(senddata.encode())
                    lastsendtime=time.time()
                lastyawerr = yawerr
                lastrollerr = rollerr
                l_lastrollerr=rollerr
                l_lastyawerr=yawerr
            # print(result2)
        if (time.time() - start) != 0:
            #print("FPS :", cny/(time.time() - start))
            cny = 0
            start = time.time()
        #print(img.shape[:2])
        #cv2.imshow("imge", imge)

        if cv2.waitKey(1) == ord(' '):
            break

    # 释放摄像头
    cap.release()
    # 销毁窗口
    cv2.destroyAllWindows()
